﻿#include "xfgraber.h"

bool XfGraber::CameraChannel::CloseCameraChannel()
{
    if (this->m_iDevID>= 0) {
        CloseDevice(m_iDevID);
        return true;
    }
    return false;
}

XfGraber::XfGraber()
{

}

bool XfGraber::enumerateDevice()
{
    try {
        DeviceName[4] = { 0 };
        int iDeviceCount = -1;
        EnumerateDevice(DeviceName, 4, &iDeviceCount);
        if (iDeviceCount == 0)
        {
            throw SilicolAbort(tr("Cannot find any grabber device!"));
        }
        for (int i = 0; i < iDeviceCount; i++)
        {
            if (DeviceName[i] != nullptr)
            {
                QString str = "[GrabberEnum] Device Found: ";
                str.append(DeviceName[i]);
                qInfo(str.toStdString().c_str());
            }
        }
        return true;
    } catch (SilicoolException &se) {
        return false;
    }

}

bool XfGraber::openDevice()
{
    try {
        m_CameraChannel.CloseCameraChannel();

        int iDevID = -1;
        if (OpenDevice(this->DeviceName[0], FALSE) != XF_ERROR_OK)
        {
            throw SilicolAbort(tr("[GrabberOpen] Open device fail!"));
        }
        BYTE pSN[32];
        int iBufferSize = 32;
        if (GetDeviceSN(pSN, iBufferSize, iDevID) == XF_ERROR_OK)
        {
            std::string s(reinterpret_cast<const char *>(pSN), 32);
            if (s.length() > 0) {
                qInfo("[GrabberOpen] Open device success!");
                m_CameraChannel.m_iDevID = iDevID;
            }
        }
        return true; //XF_ERROR_OK;
    } catch (SilicoolException &se) {
        return false; //XF_ERROR_FAILED;
    }
}

bool XfGraber::closeDevice()
{
    QElapsedTimer timer;
    timer.start();
    isGrabbing = false;
    setCurrentSensorID("");
    m_CameraChannel.CloseCameraChannel();
    qInfo("Close device! Time elapsed: %d ms.", timer.elapsed());
    return true; //XF_ERROR_OK;
}

bool XfGraber::loadIniFile()
{
    SensorTab *pCurrentSensor;
    pCurrentSensor = &(this->m_CameraChannel.current_sensor);
    iniParser *iniParser_ = new iniParser();
    QString filename = this->m_IniFilename;
    filename.replace("file:///", "");
    std::string str_filename = filename.toStdString();
    iniParser_->SetIniFilename(str_filename);

    {
        m_CameraChannel.m_fMclk = (float)iniParser_->ReadIniData("Sensor", "mclk", 0x01) / 1000;
        m_CameraChannel.m_fAvdd = (float)iniParser_->ReadIniData("Sensor", "AVDD", 0x00) / 1000;
        m_CameraChannel.m_fDovdd = (float)iniParser_->ReadIniData("Sensor", "DOVDD", 0x00) / 1000;
        m_CameraChannel.m_fDvdd = (float)iniParser_->ReadIniData("Sensor", "DVDD", 0x00) / 1000;
    }

    pCurrentSensor->ROI_X = iniParser_->ReadIniData("Sensor", "ROI_X", 0);
    pCurrentSensor->ROI_Y = iniParser_->ReadIniData("Sensor", "ROI_Y", 0);

    pCurrentSensor->width = iniParser_->ReadIniData("Sensor", "width", 0);
    pCurrentSensor->height = iniParser_->ReadIniData("Sensor", "height", 0);
    pCurrentSensor->vcheight = iniParser_->ReadIniData("Sensor", "vcheight", 0);

    pCurrentSensor->sensorFormat = iniParser_->ReadIniData("Sensor", "sensorFormat", 0);
    pCurrentSensor->SensorPort = iniParser_->ReadIniData("Sensor", "sensorPort", 0);
    pCurrentSensor->pwdn = iniParser_->ReadIniData("Sensor", "pwdn", 0);

    pCurrentSensor->SlaveID[0] = iniParser_->ReadIniData("Sensor", "SlaveID", 0);
    pCurrentSensor->SlaveID[1] = iniParser_->ReadIniData("Sensor", "SlaveID1", 0);
    pCurrentSensor->SlaveID[2] = iniParser_->ReadIniData("Sensor", "SlaveID2", 0);
    pCurrentSensor->SlaveID[3] = iniParser_->ReadIniData("Sensor", "SlaveID3", 0);
    pCurrentSensor->i2cRate = iniParser_->ReadIniData("Sensor", "I2cRate", 1);

    pCurrentSensor->i2cMode = iniParser_->ReadIniData("Sensor", "i2c_mode", 0);
    pCurrentSensor->SensorIDAddr = iniParser_->ReadIniData("Sensor", "SensorIDAddr", 0);
    pCurrentSensor->SensorID = iniParser_->ReadIniData("Sensor", "SensorID", 0);
    pCurrentSensor->PixOrder = iniParser_->ReadIniData("Sensor", "disply_mode", 0);
    pCurrentSensor->mclk = iniParser_->ReadIniData("Sensor", "mclk", 0);
    pCurrentSensor->avdd = iniParser_->ReadIniData("Sensor", "AVDD", 0);
    pCurrentSensor->dovdd = iniParser_->ReadIniData("Sensor", "DOVDD", 0);
    pCurrentSensor->dvdd = iniParser_->ReadIniData("Sensor", "DVDD", 0);
    pCurrentSensor->mipiRxMode = iniParser_->ReadIniData("Sensor", "mipiclk", 0);

    pCurrentSensor->ParaList = new USHORT[8192 * 4];
    pCurrentSensor->ParaListSize = 0;
    iniParser_->GetI2CData(pCurrentSensor);

    m_SensorTab = pCurrentSensor;
    delete iniParser_;
    return true;
}

bool XfGraber::startCamera()
{
    openSensorPower();

    SetSensorI2cRate(m_SensorTab->i2cRate, m_CameraChannel.m_iDevID);

    SetSensorClock(TRUE, m_SensorTab->mclk, m_CameraChannel.m_iDevID);

    SetPwdnRstGpio(!m_SensorTab->pwdn, !m_SensorTab->pwdn, 0, m_CameraChannel.m_iDevID);	//复位芯片
    Sleep(5);

    SetPwdnRstGpio(m_SensorTab->pwdn, !m_SensorTab->pwdn, RESET_H, m_CameraChannel.m_iDevID);	//复位芯片
    Sleep(5);

    if (readSensorID(m_SensorTab) == FALSE)
    {
        //AfxMessageBox("No Sensor!!!");
        //StopVideo();
        return FALSE;
    }
    InitSensorReg(m_SensorTab->CurrentSlaveID, m_SensorTab->i2cMode, m_SensorTab->ParaList, m_SensorTab->ParaListSize, m_CameraChannel.m_iDevID);

    SetMipiCLkLpMode(m_SensorTab->mipiRxMode, m_CameraChannel.m_iDevID); //设置工作模式,0:mipi时钟连续 1:mipi时钟不连续

    InitFPGA(m_SensorTab->ROI_X, m_SensorTab->ROI_Y, m_SensorTab->width, m_SensorTab->height + m_SensorTab->vcheight, m_SensorTab->SensorPort, m_SensorTab->sensorFormat, m_SensorTab->PixOrder, m_CameraChannel.m_iDevID);//+vcheight

    CalculateGrabSize(&m_bufLen, m_CameraChannel.m_iDevID);

    SetGrabTimeout(3500, m_CameraChannel.m_iDevID);

    return TRUE;

}

void XfGraber::openSensorPower()
{

    SENSOR_POWER power[5];
    int powerval[5];
    POWER_MODE mode[4];

    power[0] = POWER_AVDD;
    powerval[0] = m_SensorTab->avdd;
    power[1] = POWER_DVDD;
    powerval[1] = m_SensorTab->dvdd;
    power[2] = POWER_DOVDD;
    powerval[2] = m_SensorTab->dovdd;
    power[3] = POWER_AFVCC;
    powerval[3] = 2800;
    power[4] = POWER_VPP;
    powerval[4] = 2800;

    mode[0] = POWER_MODE_WORK;
    mode[1] = POWER_MODE_WORK;
    mode[2] = POWER_MODE_WORK;
    mode[3] = POWER_MODE_WORK;



    PmuSetVoltage(power, powerval, 5, m_CameraChannel.m_iDevID);//设置电压

    Sleep(50);


#ifdef POWER_SEQ
    SetSensorPowerOnOff(FALSE, m_CameraChannel.m_iDevID);//关闭电源总开关
#else
    SetSensorPowerOnOff(TRUE, m_CameraChannel.m_iDevID);//打开所有电源总开关
#endif


#ifdef POWER_SEQ     //模拟上电时序

    PmuSetOnOff(POWER_AVDD, TRUE, m_CameraChannel.m_iDevID);	//开启AVDD
    Sleep(10);
    PmuSetOnOff(POWER_DOVDD, TRUE, m_CameraChannel.m_iDevID);	//开启DOVDD
    Sleep(10);
    PmuSetOnOff(POWER_DVDD, TRUE, m_CameraChannel.m_iDevID);	//开启DVDD
    PmuSetOnOff(POWER_AFVCC, TRUE, m_CameraChannel.m_iDevID);	//开启AFVCC
    PmuSetOnOff(POWER_VPP, TRUE, m_CameraChannel.m_iDevID);	//开启AFVCC
    Sleep(10);
#endif

    PmuSetMode(power, mode, 4, m_CameraChannel.m_iDevID);//设置电源档位是工作模式

}

bool XfGraber::readSensorID(SensorTab *pSensorTab)
{
    WORD sensorID = 0;
    WORD sensorID_LSB = 0;

    byte slaveID[4] = { pSensorTab->SlaveID[0], pSensorTab->SlaveID[1], pSensorTab->SlaveID[2], pSensorTab->SlaveID[3] };

    for (int i = 0; i<4; i++)
    {
        if (slaveID[i] != 0)
        {
            if ((byte)(pSensorTab->SensorID >> 8) == 0x00) //8bit ID
            {
                ReadSensorReg(slaveID[i], pSensorTab->SensorIDAddr, &sensorID, pSensorTab->i2cMode, m_CameraChannel.m_iDevID);
            }
            else                                         //16 bit ID
            {
                if (pSensorTab->i2cMode == _16ADDR_16REG || pSensorTab->i2cMode == _8ADDR_16REG)
                {
                    ReadSensorReg(slaveID[i], pSensorTab->SensorIDAddr, &sensorID, pSensorTab->i2cMode, m_CameraChannel.m_iDevID);

                }
                else
                {
                    ReadSensorReg(slaveID[i], pSensorTab->SensorIDAddr, &sensorID, pSensorTab->i2cMode, m_CameraChannel.m_iDevID);
                    sensorID <<= 8;
                    ReadSensorReg(slaveID[i], pSensorTab->SensorIDAddr + 1, &sensorID_LSB, pSensorTab->i2cMode, m_CameraChannel.m_iDevID);
                    sensorID |= sensorID_LSB;

                }

            }
            if (sensorID == pSensorTab->SensorID)
            {
                pSensorTab->CurrentSlaveID = slaveID[i];
                return TRUE;
            }
        }
    }

    return FALSE;
}
